/*
 * Copyright (C) 2006 Roman Krashanitsa
 *
 * @author Roman Krashanitsa
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
 *
 */

package pprz.data_components;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.io.UnsupportedEncodingException;
import java.nio.ByteBuffer;
import java.util.TooManyListenersException;

import javax.comm.CommPort;
import javax.comm.CommPortIdentifier;
import javax.comm.NoSuchPortException;
import javax.comm.PortInUseException;
import javax.comm.SerialPort;
import javax.comm.SerialPortEvent;
import javax.comm.SerialPortEventListener;
import javax.comm.UnsupportedCommOperationException;
import javax.swing.event.EventListenerList;

import pprz.data_components.events.DataEvent;



public class XBee 
extends Device implements UplinkDevice, SerialPortEventListener {
	/**
	 * This class provides an interface to the XBee Wireless communication
	 * module.
	 * 
	 * @author Roman Krashanitsa, based on Jon Keinath's stamp.rf module
	 * @version 0.1 March 2007
	 */

	// ------------------------------------CONSTANTS----------------------------
	protected static final String CR = "\r"; // 0x0D;

	// Standard bauds
	public static final int baud1200 = 0;

	public static final int baud2400 = 1;

	public static final int baud4800 = 2;

	public static final int baud9600 = 3; // Default

	public static final int baud19200 = 4;

	public static final int baud38400 = 5;

	public static final int baud57600 = 6;

	// dBm Levels Default = 4
	// XBEE-PRO | XBEE
	public static final int dBm10n10 = 0; // 10dBm | -10dBm

	public static final int dBm12n6 = 1; // 12dBm | - 6dBm

	public static final int dBm14n4 = 2; // 14dBm | - 4dBm

	public static final int dBm16n2 = 3; // 16dBm | - 2dBm

	public static final int dBm18n0 = 4; // 18dBm | 0dBm
	
	// --------------------------------VARIABLES--------------------------------
	
	public int lastSender;
	public int RSSI;
	public int discarded;
	
	public int received;
	
	protected static CommPortIdentifier portId;

	protected InputStream is;
	protected OutputStream os;
	
	protected CommPort serialPort; // Communication port

//	protected static int myAddr; // Node Address in Hex

	protected static int destAddr; // Destination in Hex

	protected static int guardTime = 1000; // Guard Time
	

	protected EventListenerList listenerList;

	// ------------------------TEMPORARY VARIABLES------------------------------
	protected static int data; // Data variable
	protected static int db; // db
	protected static int buf_size = 10000;
	protected static byte buf[]=new byte[buf_size]; // Data variable
	protected int buf_start=0;
	protected int buf_end=0;
//	protected String portName;
	protected int myAddress;
	protected String name;


	/**
	 * Creates a XBee Wireless Communication Object.
	 * 
	 * @param portName
	 * 			  symbolical port name in OS
	 * @param myAddress
	 *            Current XBee Node Address
	 * @param name
	 *            Name for Current XBee Node
	 */
	public XBee(String portName, int myAddress, String name) {
		super();
		lastSender=0;
		RSSI=0;
		discarded=0;
		ok=false;
		received=0;
		this.portName=portName;
		this.myAddress=myAddress;
		this.name=name;
		listenerList = new EventListenerList();
	}
	
	public void init()   
	{
		super.init();
		ok=false;
		//enumerate comm port
		try {
			 portId = CommPortIdentifier.getPortIdentifier(portName);
		} catch (NoSuchPortException ex) {
			System.out.println("COM port "+portName+" not found..");
			ok=false;
		} 
		//open serial port
		try {
			serialPort = (SerialPort)portId.open( name, 1000);
		} catch (PortInUseException ex) {
			System.out.println("Port "+portName+" is in use..");
			//System.exit(0);
			ok=false;
		}
		
		try {
			((SerialPort)serialPort).setSerialPortParams(9600, SerialPort.DATABITS_8,SerialPort.STOPBITS_1,SerialPort.PARITY_NONE);
			((SerialPort)serialPort).setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
		} catch (UnsupportedCommOperationException ex) {
			ok=false;
		}
		//serialPort.enableReceiveThreshold(12);
		
		System.out.println("Port '" + serialPort.getName() + "' settings:");
		System.out.println("\tInput Buffer Size: " + serialPort.getInputBufferSize());
		System.out.println("\tReceive Framing Enabled? " + serialPort.isReceiveFramingEnabled());
		System.out.println("\tReceive Framing Byte: " + serialPort.getReceiveFramingByte());
		System.out.println("\tReceive Threshold Enabled? " + serialPort.isReceiveThresholdEnabled());
		System.out.println("\tReceive Threshold: " + serialPort.getReceiveThreshold());
		System.out.println("\tReceive Timeout Enabled? " + serialPort.isReceiveTimeoutEnabled());
		System.out.println("\tReceive Timeout: " + serialPort.getReceiveTimeout());
		try {
			is = serialPort.getInputStream();
			os = serialPort.getOutputStream();
		} catch (IOException ex){
			ok=false;
		}
				
		//assign listener
		try {
			((SerialPort)serialPort).addEventListener(this);
		} catch (TooManyListenersException ex) {
			System.out.println("Port is busy..");
			ok=false;
		}
		
		try {
			enableAPI();
			setAddr(myAddress); // Set Address of XBee
			setIdentify(name); // Identifier of Node
//			discover();
		} catch (IOException ex) {
			ok=false;
		}
		
		((SerialPort)serialPort).notifyOnDataAvailable(true);
		ok=true;
	}
	
	public void send(String str) throws IOException {
	      if (str != null && str.length() > 0 ) {
	            char[] chars = new char[80];
	            str.getChars(0, str.length(), chars, 0);
                System.out.println("SerialConnection.sendString=" + str);
               	os.write(str.getBytes("ASCII"));
               	os.flush();
	        }
	}

	public void send(String str, int destNode) throws IOException {
		if (destNode != destAddr) setDestNode(destNode);
		send(str); // use generic send method
	}

	public void send(StringBuffer toSend) throws IOException {
		for (int i = 0; i < toSend.length(); i++)
			// Send String Buffer
			os.write((int) toSend.charAt(i)); // Split into bytes
	}

	public void sendTX(byte[] toSend, int dest) throws IOException {
		int len=toSend.length+9;
		ByteBuffer buff=ByteBuffer.allocate(len);
		buff.put((byte)0x7E);
		buff.putShort((short)(len-4));
		buff.put((byte)0x01); // send RF data command
		buff.put((byte)0x0A); //frame id for future ACK
		if (dest==39)
			buff.putShort((short)0xA); //destination address
		else buff.putShort((short)dest); //destination address
		buff.put((byte)0x00); //options
		buff.put(toSend);
		add_checksum(buff.array());
		os.write(buff.array());
	}
	
	/**
	 * Sends a Int
	 * 
	 * @param toSend
	 *            an Int to send
	 */
	public void send(int num) throws IOException {
		send(Integer.toString(num));
	}

	/**
	 * Sends a StringBuffer to a specific Node.
	 * 
	 * @param str
	 *            a StringBuffer to send
	 * @param destNode
	 *            Destination Address
	 */
	public void send(StringBuffer str, int destNode) throws IOException {
		if (destNode != destAddr) setDestNode(destNode); // change destnation if
		send(str); // required
	}

	/**
	 * Set the Guard Time for entering Command Mode.
	 * 
	 * @param time
	 *            Guard Time
	 */
	public void setGuardTime(int time) throws IOException {
//		enterCmd();
//		send("ATGT ");
//		send(time);
//		send(CR);
//		waitOK();
//		exitCmd();
		guardTime = time;
		ATCmd("GT",time);
	}

	/**
	 * Set the destination Node of the XBee.
	 * 
	 * @param node
	 *            Node Address see Xbee Docs for Values
	 */
	public void setDestNode(int node) throws IOException {
//		enterCmd();
//		send("ATDL ");
//		send(node);
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("DL",node);
	}

	/**
	 * Change communication speed of XBee.
	 * 
	 * @param XbeeBaud
	 *            0-6, see CONSTANTS Above
	 * @param uartBaud
	 *            "Uart.speed#####"
	 */
	public void setXBeePortParams(int XbeeBaud, int uartBaud) 
	throws UnsupportedCommOperationException, IOException {
//		enterCmd();
//		send("ATBD ");
//		send(XbeeBaud);
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("BD",XbeeBaud);
		((SerialPort)serialPort).setSerialPortParams(uartBaud, 
									   SerialPort.DATABITS_8, 
									   SerialPort.STOPBITS_1, 
									   SerialPort.PARITY_NONE);
	}

	/**
	 * setPacketSize for Transmission
	 * 
	 * @param packetSize
	 *            See XBee docs for details
	 */
	public void setPackets(int packetSize) throws IOException {
		if (packetSize < 0xFF) {
//			enterCmd();
//			send("ATRO ");
//			send(packetSize);
//			send(CR);
//			waitOK();
//			exitCmd();
			ATCmd("RO",packetSize);
		}
	}

	/**
	 * Discover nodes in network.
	 * 
	 * Returns info to debug screen (for now)
	 */
	public void discover() throws IOException {
//		enterCmd();
//		send("ATND");
//		send(CR);
		ATCmd("ND");
//		System.out.println("\nDISCOVERING NODES...");
//		while (is.available()>0) {
//			data = (char) is.read();
//			if (data == 0x0D)
//				System.out.print("\n");
//			else
//				System.out.print((char) data);
//		}
//		exitCmd();
//		System.out.println("\n...DISCOVERY COMPLETE");
	}

	/**
	 * Set Power Level for Transmission.
	 * 
	 * @param level
	 *            0-4, see vars above for details
	 */
	public void setPowerLevel(int level) throws IOException {
//		enterCmd();
//		send("ATPL");
//		send(level);
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("PL",level);
	}

	/**
	 * Get RF Level of last Received Byte
	 * 
	 * @return RF Level of last Received Byte
	 */
	public int getRFLevel() throws IOException {
		enterCmd();
		send("ATDB");
		send(CR);
		db = 0;
		data = is.read();
		while (isDigit(data)) {
			db = 10 * db + (data - '0');
			data = is.read();
		}
		exitCmd();
		return db;
	}

	/**
	 * Save settings into nonvolital memory for use on next startup.
	 * 
	 */
	public void save() throws IOException {
//		enterCmd();
//		send("ATWR");
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("WR");
	}

	/**
	 * Set XBee back to default values.
	 * 
	 */
	public void defaults() throws IOException {
//		enterCmd();
//		send("ATRE");
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("RE");
		
	}

	/**
	 * Get Guard Time from the XBee.
	 * 
	 * @return The Guard Time Value
	 */
	public int getGuardTime() throws IOException {
		enterCmd();
		send("ATGT");
		send(CR);
		db = 0;
		data = is.read();
		while (isDigit(data)) {
			db = 10 * db + (data - '0');
			data = is.read();
		}
		exitCmd();
		return db;
	}

	/**
	 * Check if a byte is waiting in the RX Uart buffer.
	 * 
	 * @return True if byte is available
	 */
	public int bytesAvailable() throws IOException{
		return is.available();
	}

	/**
	 * Wait for a Value and Return it.
	 * 
	 * @return byte received via XBee and rx
	 */
	public int rxByte() throws IOException {
		return is.read();
	}

	public void ATCmd(String cmd, int val) throws IOException
	{
		int len=12;
		byte[] bytes; 
		try {
			bytes=cmd.getBytes("ASCII");
		} catch (UnsupportedEncodingException e) 
		{
			bytes=new byte[2];
		}
		ByteBuffer buff=ByteBuffer.allocate(len);
		buff.put((byte)0x7E);
		buff.putShort((short)(len-4));
		buff.put((byte)0x08); // AT command code
		buff.put((byte)0x08); // Frame ID for reference
		buff.put(bytes[0]); //command
		buff.put(bytes[1]); //
		buff.putInt(val); //value		
		add_checksum(buff.array());
		os.write(buff.array());
		//checkResponse();
	}
	
	public void ATCmd(String cmd, String val) throws IOException
	{
		byte[] str=val.getBytes("ASCII");
		int len=9+str.length;
		byte[] bytes; 
		try {
			bytes=cmd.getBytes("ASCII");
		} catch (UnsupportedEncodingException e) 
		{
			bytes=new byte[2];
		}
		ByteBuffer buff=ByteBuffer.allocate(len);
		buff.put((byte)0x7E);
		buff.putShort((short)(len-4));
		buff.put((byte)0x08); // AT command code
		buff.put((byte)0x08); // Frame ID for reference
		buff.put(bytes[0]); //command
		buff.put(bytes[1]); //
		buff.put(str); //value		
		add_checksum(buff.array());
		os.write(buff.array());
	}

	public void ATCmd(String cmd) throws IOException
	{
		int len=8;
		byte[] bytes; 
		try {
			bytes=cmd.getBytes("ASCII");
		} catch (UnsupportedEncodingException e) 
		{
			bytes=new byte[2];
		}
		ByteBuffer buff=ByteBuffer.allocate(len);
		buff.put((byte)0x7E);
		buff.putShort((short)(len-4));
		buff.put((byte)0x08); // AT command code
		buff.put((byte)0x08); // Frame ID for reference
		buff.put(bytes[0]); //command
		buff.put(bytes[1]); //		
		add_checksum(buff.array());
		os.write(buff.array());
	}

	// protected methods
	/**
	 * Set Identify of XBee Node.
	 * 
	 * @param name
	 *            Node Name for XBee
	 */
	public void setIdentify(String name) throws IOException {
//		enterCmd();
//		send("ATNI ");
//		send(name);
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("NI",name);
	}

	/**
	 * Enable RTS mode on XBee.
	 * 
	 */
	public void enableRTS() throws IOException {
		enterCmd();
		send("ATD6 1");
		send(CR);
		waitOK();
		exitCmd();
	}
	
	/**
	 * Set API mode XBee.
	 * 
	 */
	public void enableAPI() throws IOException {
//		enterCmd();
//		send("ATAP 1"); send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("AP",1);
//		ATQuery("AP");

	}

	/**
	 * Set TTY mode XBee.
	 * 
	 */
	public void enableTTY() throws IOException {
//		enterCmd();
//		send("ATD6 1"); send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("D6",1);
	}
	
	/**
	 * Set Node Address on XBee.
	 * 
	 * @param address
	 *            Node Address for XBee
	 */
	protected void setAddr(int address) throws IOException {
//		enterCmd();
//		send("ATMY ");
//		send(address);
//		send(CR);
//		waitOK();
//		exitCmd();
		ATCmd("MY",myAddress);
	}

	/**
	 * Enter Command Mode on XBee.
	 * 
	 */
	protected void enterCmd() throws IOException {
		try {
			Thread.sleep(guardTime); // Command Mode Guard Time
			send("+++"); // Enter Command Mode
			Thread.sleep(guardTime); // Command Mode Guard Time
			waitOK(); // wait for XBee to send 'OK'
		} catch (InterruptedException e) {
			System.out.println("Interrupted...");
		}
	}

	/**
	 * Exit Command Mode on XBee.
	 * 
	 */
	protected void exitCmd() throws IOException {
		send("ATCN"); // Exit Command Mode String
		send(CR); // <CR>
		waitOK();
	}

	/**
	 * Test if character is a digit.
	 * 
	 * @param ch
	 *            Character to be tested
	 * @return True if ch is a digit
	 */
	public static boolean isDigit(int ch) {
		return ch <= 'F' && ch >= '0'; // between 0 and 15
	}

	/**
	 * Wait for an 'OK' from the XBee
	 * 
	 */
	protected void waitOK() throws IOException {
		long start=System.currentTimeMillis(); // Clear timer for timeout chk
		int n;
		String s;
		boolean okfound=false;
		while (!okfound && System.currentTimeMillis()-start<guardTime)
		{
			while (is.available()==0 && System.currentTimeMillis()-start<guardTime)
				; // wait for 'K'
			
			if (System.currentTimeMillis()-start<guardTime)	
			{
				n=is.read(buf, 0, is.available());
				s=new String(buf, 0, n, "ASCII");
				if (s.contains("OK")) okfound=true;
				System.out.println("OK");
			}
			else {
				System.out.println("waitOK TIMEOUT");
			}
		} //timeout or okfound
	}

	public void serialEvent(SerialPortEvent ev) {
//		System.out.println("Serial Event..");
		switch(ev.getEventType()) {
			case SerialPortEvent.BI:
			case SerialPortEvent.OE:
			case SerialPortEvent.FE:
			case SerialPortEvent.PE:
			case SerialPortEvent.CD:
			case SerialPortEvent.CTS:
			case SerialPortEvent.DSR:
			case SerialPortEvent.RI:
			case SerialPortEvent.OUTPUT_BUFFER_EMPTY:
				break;
			case SerialPortEvent.DATA_AVAILABLE:
//				System.out.println("Data available..");
				try {
					buf_end+=is.read(buf, buf_end, 
									  (buf_size-buf_end)>is.available()?is.available():(buf_size-buf_end));
				} catch (IOException e) {
				// nothing	
					System.out.println("IO Exception.");
				}
				if (buf_end>=buf_size) buf_end=0;
				procBufData();
		}
	}
	
	protected void procBufData()
	{
		int i=buf_start;
		int frame_size=0;
		int crc=0;
		boolean found=false;
		//search API frame delimeter
		while (i!=buf_end && !found)
		{
			if (((int)buf[i] & 0xFF) == 0x7e) found=true;
			else { i++; if (i>=buf_size) i=0; }
		}
		if (!found) return; // frame start not found, collect more data
		if ( (buf_end<buf_start && buf_size-i+buf_end>2)
			 || (buf_end>=buf_start && buf_end-i>2)) // read frame size
		{
			int hi=0;
			int lo=0;
			i++; if (i>=buf_size) i=0;
			hi=buf[i];
			i++; if (i>=buf_size) i=0;
			lo=buf[i];
			frame_size=hi>>16 | lo;
		}
		else return; // wait for rest of data
		if (frame_size<=0) return;
		if ( (buf_end<buf_start && buf_size-i+buf_end-1>frame_size)
				 || (buf_end>=buf_start && buf_end-i-1>frame_size)) // read frame
			{
				int ind=0;
				i++; if (i>=buf_size) i=0;
				byte frame[]=new byte[frame_size];
				while (ind<frame_size && i!=buf_end)
				{
					frame[ind]=buf[i];
					crc+=(int)buf[i] & 0xFF;
					ind++; i++; if (i>=buf_size) i=0;
				}
				if (ind<frame_size) { 
					System.out.println("XBee buffer error..");
					return;
				}
				crc+=(int)buf[i] & 0xFF;
				if ((crc&0xFF)==0xFF) procFrameData(frame);
				else discarded++;
				buf_start=i+1; if (buf_start>=buf_size) buf_start=0;
			}
			else return; // wait for rest of data
		
	}
	
	protected void procFrameData(byte[] frame)
	{
		if (frame.length<2)
		{
			System.out.println("XBee frame length<2 error in procFrameData..");
			return;
		}
		int len=frame.length;
		int cmdID=frame[0];
		switch (cmdID & 0xFF) {
		case 0x8A: // Modem status
			System.out.println("Modem status");
			break;
		case 0x08: // AT Command
			System.out.println("AT Command");
			break;
		case 0x09: // AT Command - Queue Parameter Value
			System.out.println("AT Command - Queue Parameter Value");
			break;
		case 0x88: // AT Command Response
			ByteBuffer b=ByteBuffer.wrap(frame, 2, frame.length-2);
			System.out.print("AT command response:frame "+String.valueOf(frame[1]));
			System.out.print(":command=");
			String Cmd=String.valueOf((char)b.get())+String.valueOf((char)b.get());
			System.out.print(Cmd);
			byte status=b.get();
			System.out.print(":status="+((status==0)?"success":"error"));
			if (b.remaining()>0 && b.remaining()<4)
			{
				System.out.print(":returned=");			
				if (b.remaining()==1) System.out.println(b.get());
				else if (b.remaining()==2) System.out.println(b.getShort());
				else if (b.remaining()==4) System.out.println(b.getInt());
			}
			else System.out.println(":followed by "+String.valueOf(b.remaining())+" bytes.");
			
			if (Cmd.equals("ND") && b.remaining()>0) //process ND information
			{
				int MY=b.getShort();
				System.out.print("\tMY="+String.valueOf(MY));
				int SH=b.getInt();
				System.out.print("\tSH="+String.valueOf(SH));
				int SL=b.getInt();
				System.out.print("\tSL="+String.valueOf(SL));
				byte DB=b.get();				
				System.out.print("\tDB="+String.valueOf(DB));
				String name=new String(b.array(),b.position(),b.remaining());
				System.out.println("\tNI="+name);	
			}
			received=frame[1];
			break;
		case 0x00: // TX (Transmit) Request: 64-bit address
			System.out.println("TX (Transmit) Request: 64-bit address");
			break;
		case 0x01: // TX (Transmit) Request: 16-bit address
			System.out.println("TX (Transmit) Request: 16-bit address");
			break;
		case 0x89: // TX (Transmit) Status
			System.out.print("TX (Transmit) Status: "+"packet "+String.valueOf(frame[1])+" - ");
			if (frame[2]==0)
				System.out.println("success.");
			else if (frame[2]==1)
				System.out.println("all retries are expired and no ACK is received.");
			else if (frame[2]==2)
				System.out.println("CCA failure.");
			else
				System.out.println("Coordinator timed out of an indirect transmission.");
			break;
		case 0x80: // RX (Receive) Packet: 64-bit Address
			System.out.println("RX (Receive) Packet: 64-bit Address");
			break;
		case 0x81: // RX (Receive) Packet: 16-bit Address
//			System.out.printf("RX (Receive) Packet: 16-bit Address: len=%d\n",len);
			if (len<6) System.out.println("Wrong format: \'RX (Receive) Packet: 16-bit Address/'");
			else {
				lastSender=frame[1]>>16 | frame[2];
				RSSI=frame[3];
				// skip options byte (4)
				byte RFData[]=new byte[len-4];
				System.arraycopy(frame, 5, RFData, 0, len-4-1);
				fireDataEvent(new DataEvent(this,RFData));
				
			}
			break;
		}
		
	}
	// Adds the checksum byte into the last byte of the suppplied array
	protected byte add_checksum(byte[] packet)
	{
		int sum=0;
		for (int i=3; i<packet.length-1; i++) sum+=(int)packet[i] & 0xFF;		
		sum=0xFF - (sum & 0xFF); //keep one byte
		packet[packet.length-1]=(byte)(sum); //checksum
		return (byte)sum;
	}
	
}
